"""
--------------------------------------------------------------------------------
功能:机械臂逆运动学程序
日期:2025-06-07
作者:Unirobot
QQ交流群:811348489
寄语：代码中能遇见你，真好，有你，真好.
署名：有你同创智能机器人科技(北京)有限公司
--------------------------------------------------------------------------------
"""

import time
import math
import cmath

class FourDofArm:
    def __init__(self):
        self.L2 = 109
        self.L3 = 126

    def angle_to_duty_cycle(self, angle):
        return (angle/270) * 20 + 2.5

    def grap(self, grap_value):
        duty_cycle = self.angle_to_duty_cycle(grap_value)
        duty_cycle = round(duty_cycle, 2)
        print("raw:", duty_cycle)

    # 运动学计算函数
    def get_coordinate(self, x1, y1, z1):
        a = 0.0
        b = 0.0
        c = 0.0
        x = 0.0
        y = 0.0
        z = 0.0
        alpha = 0.0
        theta = 0.0
        theta1 = 0.0
        theta2 = 0.0
        beta = 0.0
        beta1 = 0.0
        gamma = 0.0

        a = 109
        b = 126
        x = x1
        y = y1
        z = z1
        # print("---------------------------------------------")
        # print("raw_x:",x, "raw_y:", y, "raw_z:", z)

        # print("input_x:",x1, "input_y:", y1, "input_z:", z1)
        c = math.sqrt(x * x + y * y + z * z)

        # print(f"c: {c}")
        theta1 = math.acos((a * a + c * c - b * b) / (2 * a * c)) * (180 / math.pi)
        # print(f"theta1: {theta1}")
        theta2 = math.atan(z / math.sqrt(x * x + y * y)) * (180 / math.pi)
        # print(f"theta2: {theta2}")
        theta = 100.9 - (90 - (theta1 + theta2))
        # print(f"theta: {theta}")
        alpha = 83 - (math.atan(y / x)) * (180 / math.pi)
        # print(f"alpha: {alpha}")
        beta1 = math.acos((a * a + b * b - c * c) / (2 * a * b)) * (180 / math.pi)
        # print(f"beta1: {beta1}")
        beta = 95 - (90 - beta1)
        # print(f"beta: {beta}")
        gamma = 141 - (180 - (beta1 + (theta1 + theta2)))
        # print(f"alpha: {alpha}")
        # print(f"theta: {theta}")
        # print(f"beta: {beta}")
        alpha = round(alpha, 2)
        theta = round(theta, 2)
        beta  = round(beta, 2)
        gamma = round(gamma, 2)

        if alpha <=0:
            alpha = 0
        if alpha >= 270:
            alpha = 0

        if theta <= 0:
            theta = 0
        if theta >= 270:
            theta = 270
        
        if beta <= 0:
            beta = 0
        if beta >= 270:
            beta = 270

        if gamma <= 0:
            gamma = 0
        if gamma >= 270:
            gamma = 270

        # print("alpha:", alpha)
        # print("theta:", theta)
        # print("beta:", beta)
        # print("gamma:", gamma)
        return (alpha, theta, beta, gamma)

    # 直线插补函数
    def linear_interpolation(self, start_x, start_y, start_z, end_x, end_y, end_z, num_points):
        for i in range(num_points + 1):
            t = i / num_points
            x = start_x + t * (end_x - start_x)
            y = start_y + t * (end_y - start_y)
            z = start_z + t * (end_z - start_z)
            self.get_coordinate(x, y, z)
            time.sleep(0.005)

    # 圆弧插补函数
    def arc_interpolation(self, start_x, start_y, end_x, end_y, radius, num_points, grap_value):
        center_x, center_y = self.calculate_circle_center(start_x, start_y, end_x, end_y, radius)
        if center_x is None:
            return

        start_angle = math.atan2(start_y - center_y, start_x - center_x)
        end_angle = math.atan2(end_y - center_y, end_x - center_x)
        delta_angle = (end_angle - start_angle) / num_points

        for i in range(num_points + 1):
            current_angle = start_angle + i * delta_angle
            x = center_x + radius * math.cos(current_angle)
            y = center_y + radius * math.sin(current_angle)
            z = 10.0
            self.get_coordinate(x, y, z)
            time.sleep(0.005)

    # 计算圆心函数
    def calculate_circle_center(self, x1, y1, x2, y2, radius):
        dx = x2 - x1
        dy = y2 - y1
        distance = math.sqrt(dx * dx + dy * dy)

        if distance > 2 * radius:
            return None, None

        mid_x = (x1 + x2) / 2
        mid_y = (y1 + y2) / 2
        chord_length = math.sqrt(radius * radius - (distance / 2) * (distance / 2))

        center_x = mid_x + chord_length * (-dy / distance)
        center_y = mid_y + chord_length * (dx / distance)

        return center_x, center_y